#include "MeasurE452AlgBackSs1Ss2Circle.h"

MeasurE452AlgBackSS1SS2Circle::MeasurE452AlgBackSS1SS2Circle(int i):MeasurE452AlgBase(i)
{

}

void MeasurE452AlgBackSS1SS2Circle::operator()(cv::Mat &srcImg,
                                               int id,
                                               std::map<string, float> &measRes,
                                               int fixtureId)
{
    qDebug() << "start to E452 alg back ss1 ss2 circle";
    //背光图小半圆边缘提取  // SS12 back图    \paras452\back\ SS.xml
//    int NewCircleEdge(cv::Mat srcimg, const vector<int> paras, vector<cv::Point>& contours);

    std::vector<std::vector<cv::Point>> resptVec;
    resptVec.resize(2);
    for(auto i=20;i<22;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-20,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;

        if(srcImg.empty()) {
            Q_ASSERT(0);
        }

        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        MeasurE452EdgeDetect obj;
        obj.NewCircleEdge(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
        while (!MeasurE452AlgBase::flagVec[id])
        {
            (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            resptVec[i-20].push_back(ptsOut);
        }
    }
    // go on coding
    //cout<<resptVec[3].x<<"\t"<<resptVec[3].y<<endl;
    MeasurE452EdgeDetect obj;
    vector<float> ss1res=obj.FitCircle(resptVec[0]);
    measRes.insert(std::make_pair(string("FAI113-SS1-X-distance_pixel"), -ss1res[0]));
    float fai113x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI113-SS1-X-distance")], -ss1res[0]);
    measRes.insert(std::make_pair(string("FAI113-SS1-X-distance_real"), fai113x));

    measRes.insert(std::make_pair(string("FAI120-SS1-Y-distance_pixel"), -ss1res[1]));
    float fai120y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI120-SS1-Y-distance")], -ss1res[1]);
    measRes.insert(std::make_pair(string("FAI120-SS1-Y-distance_real"), fai120y));

    measRes.insert(std::make_pair(string("SS1-diam_pixel"), ss1res[2]*2));
    float faiSS1 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS1-diam")], ss1res[2]*2);
    measRes.insert(std::make_pair(string("SS1-diam_real"), faiSS1));

    vector<float> ss2res=obj.FitCircle(resptVec[1]);
    measRes.insert(std::make_pair(string("FAI114-SS2-X-distance_pixel"), -ss2res[0]));
    float fai114x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI114-SS2-X-distance")], -ss2res[0]);
    measRes.insert(std::make_pair(string("FAI114-SS2-X-distance_real"), fai114x));

    measRes.insert(std::make_pair(string("FAI121-SS2-Y-distance_pixel"), -ss2res[1]));
    float fai121y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI121-SS2-Y-distance")], -ss2res[1]);
    measRes.insert(std::make_pair(string("FAI121-SS2-Y-distance_real"), fai121y));

    measRes.insert(std::make_pair(string("SS2-diam_pixel"), ss2res[2]*2));
    float faiSS2 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS2-diam")], ss2res[2]*2);
    measRes.insert(std::make_pair(string("SS2-diam_real"), faiSS2));

    qDebug() << "end to E452 alg back ss1 ss2 circle";
}
